#include <stdio.h>
#include <stdlib.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <string.h>
#include <unistd.h>
#include <iostream>
#include <ros/ros.h>
#include "roborts_msgs/multi_car.h"
#include "roborts_msgs/single_car.h"

#include "communication.h"
using namespace std;
#define BUFFER_SIZE 1024
#define INETADDR "192.168.3.201"  // 中转站server端的ip
//#define INETADDR "127.0.0.1"  // 中转站server端的ip
#define CLIENT_PORT 2346

ros::Publisher robot_recv_pub;
roborts_msgs::multi_car car_info;

class TcpClient
{
public:
    float recv_msg[BUFFER_SIZE];
    float input_msg[BUFFER_SIZE] = {1.1, 1.2};
    int server_sock_fd = 0;
    struct sockaddr_in server_addr;
    int clientInit( );
    void clientSend(float *sendMessage);
    void clientRecv();
};

int TcpClient::clientInit( )
{   
    server_addr.sin_family = AF_INET;
    server_addr.sin_port = htons(CLIENT_PORT);
    server_addr.sin_addr.s_addr = inet_addr(INETADDR);
    bzero(&(server_addr.sin_zero), 8);
   
    server_sock_fd = socket(AF_INET, SOCK_STREAM, 0);
    printf("server_sock_fd=%d\n", server_sock_fd);
    if (server_sock_fd == -1)
    {
        perror("socket error");
        return 1;
    }
    while (connect(server_sock_fd, (struct sockaddr *)&server_addr, sizeof(struct sockaddr_in)) != 0)
    {
        sleep(0.1);
        printf("connect error\n");
    }
}

void TcpClient::clientSend(float *sendMessage) //改为float
{                                                                              //printf("server_sock_fd222222=%d\n",server_sock_fd);
    int length = 2 * 4;
    //vector取出来赋值
    // if (send(server_sock_fd, inputMessage, strlen(inputMessage), 0) == -1)
    if (send(server_sock_fd, (float *)sendMessage, length, 0) == -1)
    {
        perror("发送消息出错!\n");
    }
}

void TcpClient::clientRecv( )
{
    //   if (connect(server_sock_fd, (struct sockaddr *)&server_addr, sizeof(struct sockaddr_in)) == 0)
    // {
    fd_set client_fd_set;
    struct timeval tv;
    struct message recv_message;
    ros::NodeHandle nh;
    while (ros::ok())
    {
        tv.tv_sec = 20;
        tv.tv_usec = 0;
        FD_ZERO(&client_fd_set);
        FD_SET(STDIN_FILENO, &client_fd_set);
        FD_SET(server_sock_fd, &client_fd_set);

        select(server_sock_fd + 1, &client_fd_set, NULL, NULL, &tv);
        if (FD_ISSET(server_sock_fd, &client_fd_set))
        {
            bzero(&recv_message, sizeof(struct message));
            long byte_num = recv(server_sock_fd, &recv_message, sizeof(struct message), 0);
            if (byte_num > 0)
            {
                if (byte_num > BUFFER_SIZE)
                {
                    byte_num = BUFFER_SIZE;
                }
                
    		    std::cout<<"robot recv"<<std::endl;
                
                if(recv_message.red1.stamp.data >= car_info.multicar[0].pose.header.stamp)
                {
                    car_info.multicar[0].pose.pose = recv_message.red1.pose;
                    car_info.multicar[0].pose.header.stamp = recv_message.red1.stamp.data;
                }
                if(recv_message.red2.stamp.data >= car_info.multicar[1].pose.header.stamp)
                {
                    car_info.multicar[1].pose.pose = recv_message.red2.pose;
                    car_info.multicar[1].pose.header.stamp = recv_message.red2.stamp.data;
                }
                if(recv_message.blue1.stamp.data >= car_info.multicar[2].pose.header.stamp)
                {
                    car_info.multicar[2].pose.pose = recv_message.blue1.pose;
                    car_info.multicar[2].pose.header.stamp = recv_message.blue1.stamp.data;
                }
                if(recv_message.blue2.stamp.data >= car_info.multicar[3].pose.header.stamp)
                {
                    car_info.multicar[3].pose.pose = recv_message.blue2.pose;
                    car_info.multicar[3].pose.header.stamp = recv_message.blue2.stamp.data;
                }
                
                std::cout<<ros::Time::now()<<std::endl;
                std::cout<<"blue2 info:"<<std::endl;
                std::cout<<car_info.multicar[3]<<std::endl;

                robot_recv_pub.publish(car_info);
            }
            else if (byte_num < 0)
            {
                printf("接受消息出错!\n");
            }
            else
            {
                printf("服务器端退出!\n");
                exit(0);
            }
        }
    }
    //}
}

TcpClient client;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "robot_recv");
    ros::NodeHandle n;
    robot_recv_pub = n.advertise<roborts_msgs::multi_car>("/robot_recv", 1);
    client.clientInit();
    for(int i=0;i<4;i++)
    {
        roborts_msgs::single_car new_car;
        car_info.multicar.emplace_back(new_car);
    }
    car_info.multicar[0].id=1;car_info.multicar[0].color=1;
    car_info.multicar[1].id=2;car_info.multicar[1].color=1;
    car_info.multicar[2].id=1;car_info.multicar[2].color=2;
    car_info.multicar[3].id=2;car_info.multicar[3].color=2;
    while (ros::ok())
    {
        client.clientRecv();
        // sleep(1);
    }
    close(client.server_sock_fd);
    exit(EXIT_SUCCESS);
    return 0;
}
